﻿import serial
import math
import os
import serial.tools.list_ports
# from collections import Counter
import random
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D

plist = list(serial.tools.list_ports.comports())

milePort = 'com10'
ardPort = 'com5'
filename = "posDis" + str(random.uniform(0,1))

distance = []
peak = []
keyData = []
keyDataIndexArr = []
keyDataIndex = 0
keyDataPositionArr = []
startDis = 150

def getData(peak, keyData, keyDataIndex, keyDataIndexArr, keyDataPositionArr, startDis):
    data = []
    peak = []
    ser = serial.Serial(milePort,115200,timeout=0.5)
    lock = False
    while(True):
        s = str(ser.readline())
        try:
            if s == "b\'Envelope data:\\n\'":
                while(True):
                    s = str(ser.readline())
                    s = s.replace("b\'", "")
                    s = s.replace("\\n\'", "")
                    if s == "end":
                        lock = True
                        break
                    temp = s.split()
                    data.extend(temp)
            if lock:
                dataLen = len(data)
                for p in data:
    	            p = int(p)
    	            peak.append(p)
                distance = [i for i in range(dataLen)]
                distance = np.array(distance) * 0.484
                keyDataPositionArr = getKeyData(peak, keyData, keyDataIndex, keyDataIndexArr, keyDataPositionArr, startDis)
                ser.close() 
                return distance, peak, keyDataPositionArr
        except ValueError:
                print("Test again!")
                break

def plotData(xData, yData, title, xlabel, ylabel):
    plt.title(title)
    plt.xlabel(xlabel)
    plt.ylabel(ylabel)
    plt.plot(xData, yData)

def getKeyData(peak, keyData, keyDataIndex, keyDataIndexArr, keyDataPositionArr, startDis):
    #输出最值并用文档记录
    keyDistance = float(peak.index(max(peak))) * 0.484 + float(startDis)
    # print(keyDistance)
    # dataString = str(max(peak)) + '\t' + str(keyDistance) + '\t'

    # with open(filename,"a") as f:
    #     f.write(dataString)

    if keyDataIndex > 10:
        keyData.pop(0)
        keyData.append(keyDistance)
        keyDataIndexArr.pop(0)
        keyDataIndexArr.append(keyDataIndex)
    else:
        keyData.append(keyDistance)
        keyDataIndexArr.append(keyDataIndex)
    keyDataPositionArr = np.array(keyDataIndexArr) * 1.6
    keyDataPositionArr = keyDataPositionArr.tolist()
    return keyDataPositionArr

def plotResult(keyDataPositionArr, keyData):
    # print(keyDataPositionArr)
    # print(keyData)
    plt.clf()
    plotData(keyDataPositionArr, keyData, 'pos&distance', 'Position', 'Distance')
    plt.draw()
    plt.pause(0.1)
    plt.ioff()

ser2=serial.Serial(ardPort,115200,timeout=0.5)
while True:
    s1 = str(ser2.readline())
    if len(s1) == 6:
        ser2.close()
        keyDataIndex = keyDataIndex + 1
        distance, peak, keyDataPositionArr = getData(peak, keyData, keyDataIndex, keyDataIndexArr, keyDataPositionArr, startDis)
        ser2=serial.Serial(ardPort,115200,timeout=0.5)
        plt.ion()
        plotResult(keyDataPositionArr, keyData)
        plt.ioff()